#include "typec_state_machine.h"
#include "pd_state_machine.h"
#include "typec_pd_timer.h"
#include "hal_tcpc_operation.h"
#include "insdrv_gpio.h"

//#define TYPEC_STATE_MACHINE_DBG
#ifdef TYPEC_STATE_MACHINE_DBG
#define TYPEC_STATE_LOG(...)    printf(__VA_ARGS__)
static char __attribute__((unused)) *typec_state_string[TYPEC_STATE_MAX] = {
    [TYPEC_DISABLED]       = "Disabled",
    [TYPEC_ERRORRECOVERY]  = "ErrorRecovery",
    [TYPEC_UNATTACHED_SNK] = "Unattached.SNK",
    [TYPEC_UNATTACHED_SRC] = "Unattached.SRC",
    [TYPEC_ATTACHWAIT_SNK] = "AttachWait.SNK",
    [TYPEC_ATTACHWAIT_SRC] = "AttachWait.SRC",
    [TYPEC_ATTACHED_SNK]   = "Attached.SNK",
    [TYPEC_ATTACHED_SRC]   = "Attached.SRC",
    [TYPEC_TRY_SNK]        = "Try.SNK",
    [TYPEC_TRY_WAIT_SRC]   = "TryWait.SRC",
};

static char __attribute__((unused)) *typec_vbus_string[TYPEC_VBUS_LEVEL_MAX] = {
    [TYPEC_VBUS_VSAFE0V]   = "VSafe0V",
    [TYPEC_VBUS_INVALID]   = "VBus_Invaid",
    [TYPEC_VBUS_VALID]     = "VBus_Valid",
};

static char __attribute__((unused)) *typec_cc_string[TYPEC_CC_STATUS_MAX] = {
    [TYPEC_CC_DRP_TOGGLE]  = "DRP",
    [TYPEC_CC_SRC_OPEN]    = "SRC.Open",
    [TYPEC_CC_SRC_RA]      = "SRC.Ra",
    [TYPEC_CC_SRC_RD]      = "SRC.Rd",
    [TYPEC_CC_SNK_OPEN]    = "SNK.Open",
    [TYPEC_CC_SNK_DEFAULT] = "SNK.Default",
    [TYPEC_CC_SNK_POWER_1A5] = "SNK.1A5",
    [TYPEC_CC_SNK_POWER_3A]  = "SNK.3A",
};
#else
#define TYPEC_STATE_LOG(...)    ((void)0)
#endif

struct typec_port_info tpc_port;
static bool vbus_pd_en = false;

/* power supply */
void typec_vbus_control(bool on, uint16_t out_mv, uint16_t out_ma)
{
    if (on) {
        if (vbus_pd_en) {
            charger_vbus_discharge(false);
            vbus_pd_en = false;
        }

        if (on != tpc_port.vbus_on) {
            charger_vbus_control(true);
        }
        if (out_mv != tpc_port.out_mv) {
            // TODO: only support 5V output so far
        }
        if (out_ma != tpc_port.out_ma) {
            // TODO: if need limit output current
        }

        tpc_port.out_mv = out_mv;
        tpc_port.out_ma = out_ma;
    } else {
        if (on != tpc_port.vbus_on) {
            charger_vbus_control(false);
            charger_vbus_discharge(true);
            vbus_pd_en = true;
        }

        tpc_port.out_mv = 0;
    }

    tpc_port.vbus_on = on;
    TYPEC_STATE_LOG("turn %s vbus %d\n", on ? "on" : "off", out_mv);
}

void typec_ibus_control(uint16_t in_ma)
{
    if (in_ma != tpc_port.in_ma) {
        charger_set_iindpm(in_ma);
    }

    if (vbus_pd_en) {
        charger_vbus_discharge(false);
        vbus_pd_en = false;
    }

    tpc_port.in_ma = in_ma;
    TYPEC_STATE_LOG("iindpm %d\n", in_ma);
}

static bool typec_is_sink_attached(bool need_orient)
{
    if (need_orient) {
        if (tpc_port.cc_orient == TYPEC_CC1_COMM) {
            if ((tpc_port.cc1_sts >= TYPEC_CC_SNK_DEFAULT) && 
                (tpc_port.cc1_sts <= TYPEC_CC_SNK_POWER_3A) && 
                (tpc_port.cc2_sts == TYPEC_CC_SNK_OPEN)) {
                return true;
            } else {
                return false;
            }
        } else {
            if ((tpc_port.cc2_sts >= TYPEC_CC_SNK_DEFAULT) && 
                (tpc_port.cc2_sts <= TYPEC_CC_SNK_POWER_3A) &&
                (tpc_port.cc1_sts == TYPEC_CC_SNK_OPEN)) {
                return true;
            } else {
                return false;
            }
        }
    } else {
        if (((tpc_port.cc1_sts >= TYPEC_CC_SNK_DEFAULT) && 
            (tpc_port.cc1_sts <= TYPEC_CC_SNK_POWER_3A) && 
            (tpc_port.cc2_sts == TYPEC_CC_SNK_OPEN)) || \
            ((tpc_port.cc2_sts >= TYPEC_CC_SNK_DEFAULT) && 
            (tpc_port.cc2_sts <= TYPEC_CC_SNK_POWER_3A) &&
            (tpc_port.cc1_sts == TYPEC_CC_SNK_OPEN))) {
            return true;
        } else {
            return false;
        }
    }
}

static inline bool typec_is_src_attached(bool need_orient)
{
    if (need_orient) {
        if (tpc_port.cc_orient == TYPEC_CC1_COMM) {
            if ((tpc_port.cc1_sts == TYPEC_CC_SRC_RD) && (tpc_port.cc2_sts != TYPEC_CC_SRC_RD)) {
                return true;
            } else {
                return false;
            }
        } else {
            if ((tpc_port.cc2_sts == TYPEC_CC_SRC_RD) && (tpc_port.cc1_sts != TYPEC_CC_SRC_RD)) {
                return true;
            } else {
                return false;
            }
        }
    } else {
        if (((tpc_port.cc1_sts == TYPEC_CC_SRC_RD) && (tpc_port.cc2_sts != TYPEC_CC_SRC_RD)) || \
            ((tpc_port.cc2_sts == TYPEC_CC_SRC_RD) && (tpc_port.cc1_sts != TYPEC_CC_SRC_RD))) {
            return true;
        } else {
            return false;
        }
    }
}

static bool typec_is_cc_attached(void)
{
    int ret;
    bool typec_attached = false;

    /* double check CC status for deglitch */
    ret = tcpc_get_cc_status(&tpc_port.cc1_sts, &tpc_port.cc2_sts);
    if (ret)
        return false;

    switch (tpc_port.typec_state) {
    case TYPEC_UNATTACHED_SNK:
    case TYPEC_ATTACHWAIT_SNK:
    case TYPEC_TRY_SNK:
        if (typec_is_sink_attached(false)) {
            typec_attached = true;
        };
        break;
    case TYPEC_ATTACHED_SNK:
        if (typec_is_sink_attached(true)) {
            typec_attached = true;
        };
        break;
    case TYPEC_UNATTACHED_SRC:
    case TYPEC_ATTACHWAIT_SRC:
    case TYPEC_TRY_WAIT_SRC:
        if (typec_is_src_attached(false)) {
            typec_attached = true;
        };
        break;
    case TYPEC_ATTACHED_SRC:
        if (typec_is_src_attached(true)) {
            typec_attached = true;
        };
        break;
    default:
        break;
    }

    return typec_attached;
}

/* Something to do on entry to typec new state machine */
int typec_state_transition_to(uint8_t new_state)
{
    int ret;
    uint8_t rp_lvl;

    /* Clear all events in the previous typec state */
    typec_timer_disable_all();
    tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
    tpc_port.cc_changed = false;
    tpc_port.vbus_changed = false;

    /* TODO: check the validation of typec state machine transition */

    tpc_port.typec_state_old = tpc_port.typec_state;
    tpc_port.typec_state = new_state;
#ifdef TYPEC_STATE_MACHINE_DBG
    TYPEC_STATE_LOG("[%d]**%s**\n", g_sys_ticks, typec_state_string[tpc_port.typec_state]);
#endif

    switch (new_state) {
    case TYPEC_DISABLED:
        ret = tcpc_set_cc(TCPC_ROLE_CTRL_CC_OPEN, TCPC_ROLE_CTRL_CC_OPEN);
        if (ret)
            return ret;
        break;
    case TYPEC_UNATTACHED_SNK:
        typec_vbus_control(false, 0 ,0);
        typec_ibus_control(0);
        ret |= tcpc_set_vconn(false);
        ret |= tcpc_set_cc(TCPC_ROLE_CTRL_CC_RD, TCPC_ROLE_CTRL_CC_RD);
        if (typec_is_cc_attached()) {
            // Sink detach for VBus removed, but cc is still attached
            delay_1ms(15); // equal to tPDDebounce
            if (typec_is_cc_attached()) {
                /**
                 * Special Case:
                 * Transition to attachwait.snk instead of HW auto DRP toggle for VBus Removed.
                 */
                tpc_port.typec_state = TYPEC_ATTACHWAIT_SNK;
                typec_timer_enable(TYPEC_TIMER_CC_DEBOUNCE, TYPEC_TIMER_T_CC_DEBOUNCE);
#ifdef TYPEC_STATE_MACHINE_DBG
                TYPEC_STATE_LOG("[%d]**%s**\n", g_sys_ticks, typec_state_string[tpc_port.typec_state]);
#endif
            } else {
                if (tpc_port.hw_auto_toggle) {
                    ret |= tcpc_start_drp_toggle();
                } else {
                    typec_timer_enable(TYPEC_TIMER_DRP_TRANSITION, TYPEC_TIMER_T_DRP_TRANSITION);
                }
            }
        } else {
            /* DRP(Try.SNK or Try.SRC) shall set auto HW DRP toggle */
            tcpc_set_cc(TCPC_ROLE_CTRL_CC_RD, TCPC_ROLE_CTRL_CC_RD);
            if (tpc_port.hw_auto_toggle) {
                ret |= tcpc_start_drp_toggle();
            } else {
                typec_timer_enable(TYPEC_TIMER_DRP_TRANSITION, TYPEC_TIMER_T_DRP_TRANSITION);
            }
        }
        if (ret)
            return ret;
        break;
    case TYPEC_UNATTACHED_SRC:
        ret |= tcpc_set_cc(TCPC_ROLE_CTRL_CC_RP, TCPC_ROLE_CTRL_CC_RP);
        /* DRP(Try.SNK or Try.SRC) shall set auto HW DRP toggle */
        if (tpc_port.hw_auto_toggle) {
            ret |= tcpc_start_drp_toggle();
        } else {
            typec_timer_enable(TYPEC_TIMER_DRP_TRANSITION, TYPEC_TIMER_T_DRP_TRANSITION);
        }
        if (ret)
            return ret;
    case TYPEC_ATTACHWAIT_SNK:
        typec_timer_enable(TYPEC_TIMER_CC_DEBOUNCE, TYPEC_TIMER_T_CC_DEBOUNCE);
        break;
    case TYPEC_ATTACHWAIT_SRC:
        typec_timer_enable(TYPEC_TIMER_CC_DEBOUNCE, TYPEC_TIMER_T_CC_DEBOUNCE);
        break;
    case TYPEC_ATTACHED_SNK:
        ret = tcpc_set_cc(TCPC_ROLE_CTRL_CC_RD, TCPC_ROLE_CTRL_CC_RD);
        if (tpc_port.cc1_sts == TYPEC_CC_SNK_OPEN) {
            tpc_port.cc_orient = TYPEC_CC2_COMM;
        } else if (tpc_port.cc2_sts == TYPEC_CC_SNK_OPEN) {
            tpc_port.cc_orient = TYPEC_CC1_COMM;
        }
        ret |= tcpc_set_orient(tpc_port.cc_orient);
        ret |= tcpc_get_remote_rplvl(&rp_lvl, tpc_port.cc_orient);
        if (rp_lvl == TYPEC_REMOTE_RP_3A) {
            typec_ibus_control(3000);
        } else if (rp_lvl == TYPEC_REMOTE_RP_1A5) {
            typec_ibus_control(1500);
        } else {
            typec_ibus_control(500);
        }
        if (ret)
            return ret;

        tpc_port.pd_port.data_role = PD_DATA_ROLE_UFP;
        pd_timer_enable(PD_TIMER_PE_STARTUP, PD_TIMER_T_PE_STARTUP, false);
        break;
    case TYPEC_ATTACHED_SRC:
        ret = tcpc_set_cc(TCPC_ROLE_CTRL_CC_RP, TCPC_ROLE_CTRL_CC_RP);
        typec_vbus_control(true, 5000, 3000);
        if (tpc_port.cc1_sts == TYPEC_CC_SRC_RD) {
            tpc_port.cc_orient = TYPEC_CC1_COMM;
            ret |= tcpc_set_orient(tpc_port.cc_orient);
            if (tpc_port.cc2_sts == TYPEC_CC_SRC_RA) {
                ret |= tcpc_set_vconn(true);
                tpc_port.pd_port.need_vconn = true;
                tpc_port.pd_port.vconn_role = PD_VCONN_ROLE_SRC;
            }
        } else if (tpc_port.cc2_sts == TYPEC_CC_SRC_RD) {
            tpc_port.cc_orient = TYPEC_CC2_COMM;
            ret |= tcpc_set_orient(tpc_port.cc_orient);
            if (tpc_port.cc1_sts == TYPEC_CC_SRC_RA) {
                ret |= tcpc_set_vconn(true);
                tpc_port.pd_port.need_vconn = true;
                tpc_port.pd_port.vconn_role = PD_VCONN_ROLE_SRC;
            }
        }
        if (ret)
            return ret;

        tpc_port.pd_port.data_role = PD_DATA_ROLE_DFP;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VALID)
            pd_timer_enable(PD_TIMER_PE_STARTUP, PD_TIMER_T_PE_STARTUP, false);
        break;
    case TYPEC_TRY_SNK:
        ret = tcpc_set_cc(TCPC_ROLE_CTRL_CC_RD, TCPC_ROLE_CTRL_CC_RD);
        if (ret)
            return ret;
        typec_timer_enable(TYPEC_TIMER_DRP_TRY, TYPEC_TIMER_T_DRP_TRY);
        break;
    case TYPEC_TRY_WAIT_SRC:
        ret = tcpc_set_cc(TCPC_ROLE_CTRL_CC_RP, TCPC_ROLE_CTRL_CC_RP);
        if (typec_is_cc_attached()) {
            typec_timer_enable(TYPEC_TIMER_TRYCC_DEBOUNCE, TYPEC_TIMER_T_TRYCC_DEBOUNCE);
        } else {
            typec_timer_enable(TYPEC_TIMER_DRP_TRY, TYPEC_TIMER_T_DRP_TRY);
        }
        if (ret)
            return ret;
        break;
    case TYPEC_ERRORRECOVERY:
        ret = tcpc_set_cc(TCPC_ROLE_CTRL_CC_OPEN, TCPC_ROLE_CTRL_CC_OPEN);
        ret |= tcpc_set_vconn(false);
        typec_vbus_control(false, 0, 0);
        if (ret)
            return ret;
        typec_timer_enable(TYPEC_TIMER_ERROR_RECOVERY, TYPEC_TIMER_T_ERROR_RECOVERY);
        break;
    default:
        break;
    }

    return 0;
}

static void typec_unattach_src_snk_main(void)
{
    if (tpc_port.cc_changed) {
        tpc_port.cc_changed = false;

        if (typec_is_sink_attached(false)) {
            typec_state_transition_to(TYPEC_ATTACHWAIT_SNK);
        } else if (typec_is_src_attached(false)) {
            typec_state_transition_to(TYPEC_ATTACHWAIT_SRC);
        } else {
            // shall not be here
            tcpc_start_drp_toggle();
        }
    }
}

static void typec_attachwait_snk_main(void)
{
    static bool cc_debounced = false;

    if (tpc_port.cc_changed) {
        tpc_port.cc_changed = false;
        if (typec_is_cc_attached()) {
            // Restart tCCDebounce timer if remote rp level changed
            typec_timer_disable(TYPEC_TIMER_PD_DEBOUNCE);
            typec_timer_enable(TYPEC_TIMER_CC_DEBOUNCE, TYPEC_TIMER_T_CC_DEBOUNCE);
            cc_debounced = false;
        } else {
            typec_timer_disable(TYPEC_TIMER_CC_DEBOUNCE);
            typec_timer_enable(TYPEC_TIMER_PD_DEBOUNCE, TYPEC_TIMER_T_PD_DEBOUNCE);
            cc_debounced = false;
        }
    }

    if (tpc_port.vbus_changed) {
        tpc_port.vbus_changed = false;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
            if (cc_debounced) {
                cc_debounced = false;
                typec_state_transition_to(TYPEC_ATTACHED_SNK);
                return;
            }
        }
    }

    if (tpc_port.typec_expired_timer == TYPEC_TIMER_CC_DEBOUNCE) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
            cc_debounced = false;
            typec_state_transition_to(TYPEC_ATTACHED_SNK);
            return;
        } else {
            cc_debounced = true;
        }
    } else if (tpc_port.typec_expired_timer == TYPEC_TIMER_PD_DEBOUNCE) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        cc_debounced = false;
        typec_state_transition_to(TYPEC_UNATTACHED_SRC);
        return;
    }
}

static void typec_attached_snk_main(void)
{
    if (tpc_port.cc_changed) {
        tpc_port.cc_changed = false;
        if (typec_is_cc_attached()) {
            // TODO: remote rp level changed
            typec_timer_disable(TYPEC_TIMER_PD_DEBOUNCE);
        } else {
            typec_timer_enable(TYPEC_TIMER_PD_DEBOUNCE, TYPEC_TIMER_T_PD_DEBOUNCE);
        }
    }

    if (tpc_port.vbus_changed) {
        tpc_port.vbus_changed = false;
        if (tpc_port.vbus_sts < TYPEC_VBUS_VALID) {
            typec_timer_disable(TYPEC_TIMER_PD_DEBOUNCE);
            tpc_port.pd_event = PD_EVENT_CC_DETACH;
            typec_state_transition_to(TYPEC_UNATTACHED_SNK);
            return;
        }
    }

    if (tpc_port.typec_expired_timer == TYPEC_TIMER_PD_DEBOUNCE) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        pd_timer_disable(PD_TIMER_PE_STARTUP);
        tpc_port.pd_event = PD_EVENT_CC_DETACH;
        typec_state_transition_to(TYPEC_UNATTACHED_SNK);
        return;
    }
}

static void typec_attached_src_main(void)
{
    if (tpc_port.cc_changed) {
        tpc_port.cc_changed = false;
        if (typec_is_cc_attached()) {
            typec_timer_disable(TYPEC_TIMER_SRC_DISCONNECT);
        } else {
            typec_timer_enable(TYPEC_TIMER_SRC_DISCONNECT, TYPEC_TIMER_T_SRC_DISCONNECT);
        }
    }

    if (tpc_port.vbus_changed) {
        tpc_port.vbus_changed = false;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
            pd_timer_enable(PD_TIMER_PE_STARTUP, PD_TIMER_T_PE_STARTUP, false);
        }
    }

    if (tpc_port.typec_expired_timer == TYPEC_TIMER_SRC_DISCONNECT) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        pd_timer_disable(PD_TIMER_PE_STARTUP);
        tpc_port.pd_event = PD_EVENT_CC_DETACH;
        typec_state_transition_to(TYPEC_UNATTACHED_SNK);
        return;
    }
}

static void typec_attachwait_src_main(void)
{
    static bool cc_debounced = false;

    if (tpc_port.cc_changed) {
        tpc_port.cc_changed = false;
        if (typec_is_cc_attached()) {
            // Restart cc debounce if remote Rd removed for deglitch
            typec_timer_disable(TYPEC_TIMER_PD_DEBOUNCE);
            typec_timer_enable(TYPEC_TIMER_CC_DEBOUNCE, TYPEC_TIMER_T_CC_DEBOUNCE);
            cc_debounced = false;
        } else {
            typec_timer_disable(TYPEC_TIMER_CC_DEBOUNCE);
            typec_timer_enable(TYPEC_TIMER_PD_DEBOUNCE, TYPEC_TIMER_T_PD_DEBOUNCE);
            cc_debounced = false;
        }
    }

    if (tpc_port.vbus_changed) {
        tpc_port.vbus_changed = false;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V) {
            if (cc_debounced) {
                cc_debounced = false;
                typec_state_transition_to(TYPEC_TRY_SNK);
                return;
            }
        }
    }

    if (tpc_port.typec_expired_timer == TYPEC_TIMER_CC_DEBOUNCE) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V) {
            cc_debounced = false;
            typec_state_transition_to(TYPEC_TRY_SNK);
            return;
        } else {
            cc_debounced = true;
        }
    } else if (tpc_port.typec_expired_timer == TYPEC_TIMER_PD_DEBOUNCE) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        cc_debounced = false;
        typec_state_transition_to(TYPEC_UNATTACHED_SNK);
        return;
    }
}

static void typec_try_snk_main(void)
{
    static bool wait_time_expired = false, is_attached = false, cc_debounced = false;
    bool tmp;

    if (tpc_port.typec_expired_timer == TYPEC_TIMER_DRP_TRY) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        wait_time_expired = true;
        is_attached = typec_is_cc_attached();
        typec_timer_enable(TYPEC_TIMER_TRYCC_DEBOUNCE, TYPEC_TIMER_T_TRYCC_DEBOUNCE);
    }

    if (wait_time_expired) {
        if (tpc_port.cc_changed) {
            tpc_port.cc_changed = false;
            tmp = typec_is_cc_attached();
            if (is_attached != tmp) {
                is_attached = tmp;
                cc_debounced = false;
                // Restart tTryCCDebounce
                typec_timer_disable(TYPEC_TIMER_TRYCC_DEBOUNCE);
                typec_timer_enable(TYPEC_TIMER_TRYCC_DEBOUNCE, TYPEC_TIMER_T_TRYCC_DEBOUNCE);
            }
        }

        if (tpc_port.vbus_changed) {
            tpc_port.vbus_changed = false;
            if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
                if (cc_debounced) {
                    wait_time_expired = is_attached = cc_debounced = false;
                    typec_state_transition_to(TYPEC_ATTACHED_SNK);
                    return;
                }
            }
        }

        if (tpc_port.typec_expired_timer == TYPEC_TIMER_TRYCC_DEBOUNCE) {
            tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
            if (is_attached) {
                if (tpc_port.vbus_sts == TYPEC_VBUS_VALID) {
                    wait_time_expired = is_attached = cc_debounced = false;
                    typec_state_transition_to(TYPEC_ATTACHED_SNK);
                    return;
                } else {
                    cc_debounced = true;
                }
            } else {
                wait_time_expired = is_attached = cc_debounced = false;
                typec_state_transition_to(TYPEC_TRY_WAIT_SRC);
                return;
            }
        }
    } else {
        /* ignore all vbus and cc event before tDRPTry */
        if (tpc_port.cc_changed)
            tpc_port.cc_changed = false;

        if (tpc_port.vbus_changed)
            tpc_port.vbus_changed = false;
    }
}

static void typec_try_wait_src_main(void)
{
    static bool cc_debounced = false;

    if (tpc_port.cc_changed) {
        tpc_port.cc_changed = false;
        if (typec_is_cc_attached()) {
            typec_timer_disable(TYPEC_TIMER_DRP_TRY);
            typec_timer_enable(TYPEC_TIMER_TRYCC_DEBOUNCE, TYPEC_TIMER_T_TRYCC_DEBOUNCE);
            cc_debounced = false;
        } else {
            typec_timer_disable(TYPEC_TIMER_TRYCC_DEBOUNCE);
            typec_timer_enable(TYPEC_TIMER_DRP_TRY, TYPEC_TIMER_T_DRP_TRY);
            cc_debounced = false;
        }
    }

    if (tpc_port.vbus_changed) {
        tpc_port.vbus_changed = false;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V) {
            if (cc_debounced) {
                cc_debounced = false;
                typec_state_transition_to(TYPEC_ATTACHED_SRC);
                return;
            }
        }
    }

    if (tpc_port.typec_expired_timer == TYPEC_TIMER_TRYCC_DEBOUNCE) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        if (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V) {
            cc_debounced = false;
            typec_state_transition_to(TYPEC_ATTACHED_SRC);
            return;
        } else {
            cc_debounced = true;
        }
    } else if (tpc_port.typec_expired_timer == TYPEC_TIMER_DRP_TRY) {
        tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
        cc_debounced = false;
        typec_state_transition_to(TYPEC_UNATTACHED_SNK);
        return;
    }
}

static void typec_run_state_machine(void)
{
    switch (tpc_port.typec_state) {
    case TYPEC_UNATTACHED_SNK:
        if (tpc_port.hw_auto_toggle) {
            typec_unattach_src_snk_main();
        } else {
            if (tpc_port.cc_changed && typec_is_cc_attached()) {
                tpc_port.cc_changed = false;
                typec_state_transition_to(TYPEC_ATTACHWAIT_SNK);
            } else if (tpc_port.typec_expired_timer == TYPEC_TIMER_DRP_TRANSITION) {
                tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
                typec_state_transition_to(TYPEC_UNATTACHED_SRC);
            }
        }
        break;
    case TYPEC_ATTACHWAIT_SNK:
        typec_attachwait_snk_main();
        break;
    case TYPEC_UNATTACHED_SRC:
        if (tpc_port.hw_auto_toggle) {
            typec_unattach_src_snk_main();
        } else {
            if (tpc_port.cc_changed && typec_is_cc_attached()) {
                tpc_port.cc_changed = false;
                typec_state_transition_to(TYPEC_ATTACHWAIT_SRC);
            } else if (tpc_port.typec_expired_timer == TYPEC_TIMER_DRP_TRANSITION) {
                tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
                typec_state_transition_to(TYPEC_UNATTACHED_SNK);
            }
        }
        break;
    case TYPEC_ATTACHWAIT_SRC:
        typec_attachwait_src_main();
        break;
    case TYPEC_ATTACHED_SNK:
        typec_attached_snk_main();
        break;
    case TYPEC_ATTACHED_SRC:
        typec_attached_src_main();
        break;
    case TYPEC_TRY_SNK:
        typec_try_snk_main();
        break;
    case TYPEC_TRY_WAIT_SRC:
        typec_try_wait_src_main();
        break;
    case TYPEC_ERRORRECOVERY:
        if (tpc_port.typec_expired_timer == TYPEC_TIMER_ERROR_RECOVERY) {
            tpc_port.typec_expired_timer = TYPEC_PD_TIMER_NONE;
            typec_state_transition_to(TYPEC_UNATTACHED_SNK); // Try.SNK
        }
        break;
    default:
        break;
    }

    return;
}

static void typec_dev_init(void)
{
    memset(&tpc_port, 0, sizeof(struct typec_port_info));
    typec_pd_timer_init();

    tpc_port.local_rplvl = TCPC_ROLE_CTRL_RP_3A;
    tpc_port.rp_level = tpc_port.local_rplvl;
    tpc_port.hw_auto_toggle = true;
#ifdef TYPEC_STATE_MACHINE_DBG
    TYPEC_STATE_LOG("typec port: [hw_auto_toggle]%d [local_rp_level]%d\n",
                    tpc_port.hw_auto_toggle, tpc_port.local_rplvl);
#endif

    pd_port_init();
}

static int typec_role_init(void)
{
    int ret;

    /* Set PD_nINT GPIO input mode */
    InsDrv_Gpio_SetMode(INSDRV_GPIO_PORTC, INSDRV_GPIO_PIN_7, INSDRV_GPIO_MODE_INPUT);
    InsDrv_Gpio_SetPullCtrl(INSDRV_GPIO_PORTC, INSDRV_GPIO_PIN_7, INSDRV_GPIO_PULLUP);

    ret = typec_state_transition_to(TYPEC_UNATTACHED_SNK);
    if (ret)
        return ret;

    return 0;
}

bool typec_check_alert(void)
{
    if (InsDrv_Gpio_GetPinLevel(INSDRV_GPIO_PORTC, INSDRV_GPIO_PIN_7) == INSDRV_GPIO_LEVEL_LOW)
        return true;
    else
        return false;
}

int typec_port_init(void)
{
    int ret;

    typec_dev_init();

    ret = charger_hw_init();
    if (ret)
        return ret;

    ret = tcpc_hw_init(&tpc_port);
    if (ret)
        return ret;

    ret = typec_role_init();
    if (ret)
        return ret;

    /* for debug */
//    delay_1ms(10);
//    dump_tcpc_registers();

    return 0;
}

static bool typec_ignore_cc_change(void)
{
    if ((tpc_port.typec_state == TYPEC_DISABLED) || (tpc_port.typec_state == TYPEC_ERRORRECOVERY)) {
        return true;
    }

    if (tpc_port.pd_pe.in_pr_swap) {
//        TYPEC_STATE_LOG("ignore cc change\n");
        return true;
    }

    return false;
}

static bool typec_ignore_vbus_change(void)
{
    if ((tpc_port.pd_pe.in_pr_swap) || (tpc_port.pd_pe.in_hard_reset)) {
//        TYPEC_STATE_LOG("ignore vbus change\n");
        return true;
    }

    return false;
}

static void typec_main_task(void)
{
    int ret;
    uint32_t alert_status;

    /* check tcpc alert and record irq */
    if (typec_check_alert()) {
        ret = tcpc_get_alert_status(&alert_status);
        if (ret)
            return;

        if (alert_status & TCPC_ALERT_FAULT) {
            /* TODO */
            tcpc_alert_status_clear(TCPC_ALERT_FAULT);
        }

        if (alert_status & TCPC_ALERT_CC_STATUS) {
            tcpc_alert_status_clear(TCPC_ALERT_CC_STATUS);
            tcpc_get_cc_status(&tpc_port.cc1_sts, &tpc_port.cc2_sts);
#ifdef TYPEC_STATE_MACHINE_DBG
            TYPEC_STATE_LOG("cc1 %s cc2 %s\n", typec_cc_string[tpc_port.cc1_sts], typec_cc_string[tpc_port.cc2_sts]);
#endif
            if (!typec_ignore_cc_change()) {
                tpc_port.cc_changed = true;
            }
        }

        if (alert_status & (TCPC_ALERT_POWER_STATUS | TCPC_ALERT_VENDOR_VSAFE0V)) {
            tcpc_alert_status_clear(TCPC_ALERT_POWER_STATUS | TCPC_ALERT_VENDOR_VSAFE0V | \
                                    TCPC_ALERT_VENDOR_DEFINED);
            tcpc_get_vbus_status(&tpc_port.vbus_sts);
#ifdef TYPEC_STATE_MACHINE_DBG
            TYPEC_STATE_LOG("vbus level %s\n", typec_vbus_string[tpc_port.vbus_sts]);
#endif
            if (!typec_ignore_vbus_change()) {
                tpc_port.vbus_changed = true;
            }

            if (((tpc_port.pd_pe.pe_state == PD_PE_SNK_DISCOVERY) || \
                 (tpc_port.pd_pe.pe_state == PD_PE_PRS_SNK_SRC_SOURCE_ON) || \
                 (tpc_port.pd_pe.pe_state == PD_PE_SRC_TRANSITION_TO_DEFAULT)) && \
                 (tpc_port.vbus_sts == TYPEC_VBUS_VALID)) {
                tpc_port.pd_event = PD_EVENT_VBUS_VSAFE5V;
            }
            if (((tpc_port.pd_pe.pe_state == PD_PE_PRS_SRC_SNK_TRANSITION_TO_OFF) || \
                 (tpc_port.pd_pe.pe_state == PD_PE_SNK_TRANSITION_TO_DEFAULT)) && \
                 (tpc_port.vbus_sts == TYPEC_VBUS_VSAFE0V)) {
                tpc_port.pd_event = PD_EVENT_VBUS_VSAFE0V;
            }
        }
    }

    tpc_port.typec_expired_timer = typec_expired_timer();
    /* typec timer, cc change, vbus event, */
    if (tpc_port.cc_changed || tpc_port.vbus_changed || tpc_port.typec_expired_timer) {
        typec_run_state_machine();
    }
}

void typec_pd_main_task(void)
{
    typec_main_task();
    pd_main_task();
    pd_dpm_main_task();
}
